import com.cyberbotics.webots.controller.DifferentialWheels;
import com.cyberbotics.webots.controller.DistanceSensor;
import com.cyberbotics.webots.controller.LightSensor;

public class BangBangLove extends DifferentialWheels {
	private static final int THRESHOLD = 3700;
	private static final int MAKELOVEAT = 2800;

	private static int TIME_STEP = 16;

	private static int S_FRONT_LEFT = 1; // Sensor front left
	private static int S_FRONT_RIGHT = 2; // Sensor front right
	private static int MIN_SPEED = 0; // min. motor speed
	private static int MAX_SPEED = 1000; // max. motor speed

	private static int CARE_LOW = 400; // min. motor speed
	private static int CARE_HIGH = 600; // max. motor speed

	private DistanceSensor[] sensors; // Array with all distance sensors
	private LightSensor[] _lightSensors; // Array with all light sensors

	/**
	 * Constructor
	 */
	public BangBangLove() {
		super();
		initSensors();
	}

	protected void initSensors() {
		// get distance sensors and save them in array
		sensors = new DistanceSensor[] { getDistanceSensor("ps5"), getDistanceSensor("ps7"), getDistanceSensor("ps0"), getDistanceSensor("ps2") };
		for (int i = 0; i < 4; i++) {
			sensors[i].enable(10);
		}
		_lightSensors = new LightSensor[4];
		// get lightSensor and assign that sensor to the position in the list
		_lightSensors[0] = getLightSensor("ls5");
		_lightSensors[1] = getLightSensor("ls7");
		_lightSensors[2] = getLightSensor("ls0");
		_lightSensors[3] = getLightSensor("ls2");
		for (LightSensor ls : _lightSensors) {
			ls.enable(100);
		}
	}

	/**
	 * Robot drives to the right
	 */
	private void driveRight() {
		setSpeed(CARE_HIGH, CARE_LOW);
	}

	/**
	 * Robot drives to the left
	 */
	private void driveLeft() {
		setSpeed(CARE_LOW, CARE_HIGH);
	}

	private void turnLeft() {
		setSpeed((-1) * CARE_LOW, CARE_HIGH);
	}

	/**
	 * Robot drives forward
	 */
	private void driveForward() {
		setSpeed(MAX_SPEED, MAX_SPEED);
	}

	/**
	 * Robot stop
	 */
	private void stop() {
		setSpeed(MIN_SPEED, MIN_SPEED);
		System.out.println("stopped");
	}

	private void align() {
		boolean hasStopped = false;
		while (step(TIME_STEP) != -1) {
			// Stop the robot
			if (hasStopped) {
				break;
			}
			// Hold position close to the light
			else if (_lightSensors[S_FRONT_LEFT].getValue() < MAKELOVEAT || _lightSensors[S_FRONT_RIGHT].getValue() < MAKELOVEAT) {
				stop();
				hasStopped = true;
			}
			// search the light-source
			else if (_lightSensors[S_FRONT_LEFT].getValue() >= THRESHOLD) {
				turnLeft();
			}
			// if light is in the left front of the robot drive left
			else if (_lightSensors[S_FRONT_LEFT].getValue() <= _lightSensors[S_FRONT_RIGHT].getValue()) {
				driveLeft();
			}
			// if light is in the right front of the robot drive right
			else if (_lightSensors[S_FRONT_LEFT].getValue() > _lightSensors[S_FRONT_RIGHT].getValue()) {
				driveRight();
			}
			// else drive forward
			else {
				driveForward();
			}
		}
	}

	/**
	 * Main method - in this method an instance of the controller is created and the method to launch the robot is called.
	 *
	 * @param args
	 */
	public static void main(String[] args) {
		BangBangLove controller = new BangBangLove();
		// Search the light as first state
		controller.align();
	}

}